#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/openni2_grabber.h>
#include <pcl/common/impl/io.hpp>  
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/time.h>
#include <pcl/surface/impl/mls.hpp>
#include <pcl/surface/mls.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/region_growing.h>

#include <iostream>

#include <string.h>
using namespace std;

typedef pcl::PointXYZ PointT;

pcl::PassThrough<PointT> pass;                         //直通滤波对象
pcl::VoxelGrid<PointT> sor;

pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;        //法线估计对象
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne1;
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象
pcl::ExtractIndices<PointT> extract;                   //点提取对象
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
pcl::EuclideanClusterExtraction<PointT> ec;

pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);

//pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());
pcl::console::TicToc tt;

char data[8];
short x, z = 0.5;
float y_min = 0.265;

class SimpleOpenNIViewer
{
public:
	SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer") {}  // Construct a cloud viewer, with a window name


														   // 定义回调函数cloud_cb_,获取到数据时对数据进行处理
	void cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
	{
		if (!viewer.wasStopped())
		{
			tt.tic();
			int times = 0;
			short pre_data[10][4];//0 x;1 z;2 size;  int *p

								  // Build a passthrough filter to remove spurious NaNs

			pass.setInputCloud(cloud);
			pass.setFilterFieldName("y");
			pass.setFilterLimits(0, 0.2);
			pass.filter(*cloud_filtered);

			sor.setInputCloud(cloud_filtered);
			sor.setLeafSize(0.003f, 0.003f, 0.003f);
			sor.filter(*cloud_filtered);


			//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
			pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
			pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
			pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
			normal_estimator.setSearchMethod(tree);
			normal_estimator.setInputCloud(cloud_filtered);
			normal_estimator.setKSearch(10);
			normal_estimator.compute(*normals);


			pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
			std::vector<pcl::PointIndices> cluster_indices;
			reg.setMinClusterSize(1500);
			reg.setMaxClusterSize(2500);
			reg.setSearchMethod(tree);
			reg.setNumberOfNeighbours(10);
			reg.setInputCloud(cloud_filtered);
			//reg.setIndices (indices);
			reg.setInputNormals(normals);
			reg.setSmoothnessThreshold(8 / 180.0 * M_PI);
			reg.setCurvatureThreshold(1);
			reg.extract(cluster_indices);


			pcl::PointCloud<PointT>::Ptr all_cloud_cluster(new pcl::PointCloud<PointT>);
			all_cloud_cluster->resize(cloud_filtered->size());

			for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
			{
				pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);

				for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
					cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
				cloud_cluster->width = cloud_cluster->points.size();
				cloud_cluster->height = 1;
				cloud_cluster->is_dense = true;
				cout << cloud_cluster->size() << endl;
				pcl::MomentOfInertiaEstimation <PointT> feature_extractor;
				feature_extractor.setInputCloud(cloud_cluster);
				feature_extractor.compute();

				pcl::PointXYZ min_point_AABB;
				pcl::PointXYZ max_point_AABB;
				feature_extractor.getAABB(min_point_AABB, max_point_AABB);

				if ((max_point_AABB.x - min_point_AABB.x) > 0.20 && (max_point_AABB.x - min_point_AABB.x) < 0.26)
				{
					all_cloud_cluster->operator+=(*cloud_cluster);
					pre_data[times][0] = min_point_AABB.x*1000;
					pre_data[times][1] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
					pre_data[times][2] = max_point_AABB.x * 1000;
					pre_data[times][3] = min_point_AABB.z * 1000;
					cout << "all(size,x,weight,z,angle):" << cloud_cluster->size()<<"   "<< pre_data[times][1] << "  " << max_point_AABB.z - min_point_AABB.z << "  " << pre_data[times][2] << "  " << pre_data[times][3] << "  " << atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) << endl;
   					times++;
				}
			}

			if (times != 1)
			{
				x = z = 0;
			}
			else if (times == 1)
			{
				x = pre_data[0][1];
				z = pre_data[0][3];
			}
		/*	else
			{
				short temp_x, temp_z, temp_size, temp_min_size, max_n, min_n;
				temp_x = temp_z = temp_size = temp_min_size = max_n = min_n = 0;
				for (int i = 0; i < times; i++)
				{
					if (pre_data[i][2] > temp_size)
					{
						temp_min_size = temp_size;
						min_n = max_n;
						temp_size = pre_data[i][2];
						max_n = i;
					}
				}
				if (temp_size - temp_min_size > 300)
				{
					x = pre_data[max_n][0];
					z = pre_data[max_n][1];
				}
				else
				{
					if (abs(pre_data[max_n][0]) < abs(pre_data[min_n][0]))
					{
						x = pre_data[max_n][0];
						z = pre_data[max_n][1];
					}
					else
					{
						x = pre_data[min_n][0];
						z = pre_data[min_n][1];
					}
				}
			}*/
			std::cout << x/10 << "  " << z/10;

			std::cerr << "  end  Done: " << tt.toc() << " ms\n\n";
			//system("pause");
			if (all_cloud_cluster->size()>0)viewer.showCloud(all_cloud_cluster);
		}
	}

	void run()
	{
		// create a new grabber for OpenNI devices
		pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

		// make callback function from member function
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
			boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

		// connect callback function for desired signal
		boost::signals2::connection c = interface->registerCallback(f);

		// start receiving point clouds
		interface->start();

		while (!viewer.wasStopped())
		{
			boost::this_thread::sleep(boost::posix_time::seconds(0.001));
		}
		/*
		while (!viewer.wasStopped()&&flag!=1)
		{
		boost::this_thread::sleep(boost::posix_time::seconds(1));
		}
		*/
		// Stop the data acquisition
		interface->stop();
	}

	pcl::visualization::CloudViewer viewer;
};


int main()
{
	//Sleep(10000);
	SimpleOpenNIViewer v;

	v.run();

	return 0;
}